导航方案设计 导航方法分为三个部分——路径规划、实时定位、轨迹纠正。

主要有三个工作任务：利用二维码定位对 AGV 初始位姿 进行矫正，保证 AGV 初始运动时其运动轨迹能够与道路平行；利用 ORBSLAM 定位能够实时的获得 AGV 在车库中的位姿；当 AGV 接近车位时，利 用二维码再次对 AGV 的位姿进行矫正，保证 AGV 能够准确的进入车位。定 位模块按照内容分一共分为两部分，二维码视觉定位和 ORB-SLAM 定位。 二维码视觉定位模块中，使用 ARToolKitPlus 库提供的二维码。在二维码识 别的过程中，利用模板匹配提高了 ID 识别的速度，在定位计算中利用 RPP 算法消除了局部最小值带来的错误解。ORB-SLAM 系统构建的是稀疏点云， 具有良好的实时性。在系统搭建的过程中使用跟踪法、局部地图、词袋法等， 多次优化保证了定位精度。同时系统加入了闭环检测，保证系统在大场景、 长时间建图定位的鲁棒性。

为了提高定位模块的实时性，我们采用构建 ORB 特征的稀疏点云地图。 但稀疏点云地图不具备几个特征，不能像传统栅格地图可以进行路径规划。 为了解决这个问题，我们采用后台通信模式。后台根据施工图纸等信息建立 车库的全局栅格地图，在全局栅格地图中进行路径规划，将路径信息通过通 信的方式下发给定位导航层，对路径信息进行动作解析，得到相应的运动控 制命令。底层对运控命令的解析，得到电机转速从而实现动作反应。这种路 径规划模式存在一个新问题，全局地图要与 ORB 稀疏点云地图之间建立准 确的联系，确保定位信息、路径信息坐标系之间转换的正确性。所以在 AGV 执行导航任务前，借助二维码辅助信息完成建立坐标系关系的建立。对已知 世界坐标的二维码识别，获得 AGV 的初始位姿并对其进行矫正。确保 AGV 初始行走的初始轨迹平行坐标轴。在 SLAM 系统初始化完成时，通过记录当 帧中相机在二维码坐标系中的位姿，从而获得世界坐标系到稀疏点云坐标系 的变换矩阵。二维码在世界坐标系之中的姿态通过人工示教法获得。

将后台发过来的全局路径以坐标点对序列的形式下发过来，根据我们应用场景及相关算法 。对这些点对进行划分，将全局路径划分为各段直线的局部路径，并根据 局部路径信息进行动作解析得到速度控制命令。由于机械误差、路面不平等 多方面原因，小车不能严格的按我们预期的轨迹行进，会发生偏离。为了保 证 AGV 在运动过程中不偏离预期的轨迹路线，需要对轨迹进行纠正。利用 ORB-SLAM 视觉的实时定位信息与理想预期轨迹对比，根据理想位置和当前 位置可以得到 AGV 的轨迹偏离误差值，针对误差设计控制律使误差值收敛， 最终求解出小车的线速度和角速度使小车向理想轨迹靠拢。由于底层的一个 响应周期内可能完不成 AGV 轨迹跟踪控制预期的动作，在 AGV 运动的过程 中轨迹偏离误差值一直都存在并且随时间的成收敛趋势，所以理论上讲 AGV 在运动的过程中在不断逼近预期轨迹，其收敛效果与底层硬件反应，算法参数的选择有直接关系。在真实应用场景里，AGV 行进的路径都为直线，且在运动前通过二维码对 AGV 的姿态进行矫正，所以真实轨迹与理想轨迹之间 的误差值不会太大。